import math
class BasicTrigo(object):
    
    @staticmethod
    def dot(A,B):
        return A[0]*B[0]+B[1]*A[1]
    
    @staticmethod
    def norm(A):
        return math.sqrt(A[0]**2+A[1]**2)

    @staticmethod
    def norm2(A):
        return A[0]**2+A[1]**2
    
    @staticmethod
    def  sub(A,B):
        C=[0,0]
        C[0]=A[0]-B[0]
        C[1]=A[1]-B[1]
        return C
    
    @staticmethod
    def  add(A,B):
        C=[0,0]
        C[0]=B[0]+A[0]
        C[1]=B[1]+A[1]
        return C
    
    @staticmethod
    def  scalar(A,f):
        C=[0,0]
        C[0]=A[0]*f
        C[1]=A[1]*f
        return C
    
    @staticmethod
    def normalizedCopy(A):
        n=BasicTrigo.norm(A)
        C=[0,0]
        C[0]=A[0]/n
        C[1]=A[1]/n
        return C
    
    @staticmethod
    def  project(A,B,C):
        AB=BasicTrigo.sub(B,A)
        AC=BasicTrigo.sub(C,A)
        
        
        
        rAB=BasicTrigo.norm2(AB)
        
        if rAB!=0:
            r=BasicTrigo.dot(AC,AB)
            r/=rAB
            para=BasicTrigo.add(A,BasicTrigo.scalar(AB,r))
        
            perp=BasicTrigo.sub(AC,para)
        
        else :
            perp=(0.,0.)
            para=AC    
        
        return (para,perp)
        
        
class Body(BasicTrigo):
    
    
    def __init__(self):
        self.position=[0.,0.]
        self.acceleration=[0.,0.]
        self.rotAcc=0.
        self.speed=[0.,0.]
        self.setMass(1.)
        self.setMomentum(1.)
        
        self.angle=0.
        self.omega=0.
        
    def resetAcc(self):
        self.acceleration=[0.,0.]
        self.rotAcc=0.
        
        
    
    def setMass(self,m):
        self.mass=float(m)
    
    def setMomentum(self,m):
        self.momentum=float(m)
        
    def setPosition(self,pos):
        self.position[0]=float(pos[0])
        self.position[1]=float(pos[1])
    
    def getPosition(self,pos):
        return position
    
    def setPosition(self,pos):
        self.position[0]=float(pos[0])
        self.position[1]=float(pos[1])
    
    def getPosition(self,pos):
        return position
            
    def applyForce(self,F,P):
        OP=self.sub(P,self.position)
        
        para,perp=self.project(self.position,P,C)
        print para
        print perp
        self.acceleration=self.add(self.acceleration,self.scalar(para,1/self.mass))
        r=self.norm(OP)
        self.rotAcc+=r*self.norm(perp)/self.momentum
        
        print self.acceleration
        print self.rotAcc
        
    def step(dt):
        self.speed=self.add(self.speed,self.scalar(self.acceleration,dt))
        self.omega+=self.rotAcc*dt
        self.position2=self.add(self.position,self.speed)
        self.angle2+=self.omega*dt
        
        #check collision
        self.setPosition(self.position2)
        self.setAngle(self.angle2)
        

if __name__=="__main__":
    A=[0.,0.]
    B=[5.,0.]
    C=[3.,1.]
    
    b=Body()
    b.applyForce(C,B)
    
